% 初始化问题参数

num_robots = 3; % 机器人数量
num_targets = 8; % 目标点数量
start_point = [0, 0, 0]; % 起始点坐标
targets = rand(num_targets, 3); % 生成随机的目标点坐标

% 初始化算法参数
max_iterations = 100; % 最大迭代次数
population_size = 20; % 群体数量

% 初始化种群
population = cell(population_size, 1);
for i = 1:population_size
    population{i} = randperm(num_targets * num_robots);
end

% 主循环
best_fitness = inf; % 最佳适应度初始值
best_solution = []; % 最佳解初始值
convergence_curve = zeros(max_iterations, 1); % 收敛曲线
for iteration = 1:max_iterations
    % 评估适应度
    fitness = zeros(population_size, 1);
    for i = 1:population_size
        solution = population{i};
        fitness(i) = calculate_fitness(solution, targets, start_point, num_robots);
        if fitness(i) < best_fitness
            best_fitness = fitness(i);
            best_solution = solution;
        end
    end
    
    % 更新收敛曲线
    convergence_curve(iteration) = best_fitness;
    
    % 选择操作
    selected_population = selection(population, fitness);
    
    % 交叉操作
    crossed_population = crossover(selected_population);
    
    % 变异操作
    mutated_population = mutation(crossed_population);
    
    % 更新种群
    population = mutated_population;
end

% 绘制收敛曲线
figure;
plot(1:max_iterations, convergence_curve, 'b');
xlabel('Iterations');
ylabel('Objective Function');
title('Convergence Curve');

% 绘制最佳解路径
figure;
hold on;
for i = 1:num_robots
    robot_path = [start_point; targets(best_solution(i:num_robots:end), :); start_point];
    plot3(robot_path(:, 1), robot_path(:, 2), robot_path(:, 3), '-o');
end
hold off;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('Best Solution');

% 计算适应度函数
% 计算适应度函数
% 计算适应度函数
function fitness = calculate_fitness(solution, targets, start_point, num_robots)
    fitness = 0;
    for i = 1:num_robots
        robot_indices = i:num_robots:numel(solution);
        robot_path = [start_point; targets(solution(robot_indices), :); start_point];
        fitness = fitness + sum(pdist(robot_path));
    end
end



% 选择操作
function selected_population = selection(population, fitness)
    [~, idx] = sort(fitness);
    selected_population = population(idx(1:ceil(end/2)));
end

% 交叉操作
function crossed_population = crossover(selected_population)
    population_size = numel(selected_population);
    crossed_population = cell(population_size, 1);
    for i = 1:population_size
        parent1 = selected_population{i};
        parent2 = selected_population{randi(population_size)};
        child = [parent1(1:floor(end/2)), parent2(floor(end/2)+1:end)];
        crossed_population{i} = child;
    end
end

% 变异操作
function mutated_population = mutation(crossed_population)
    population_size = numel(crossed_population);
    mutated_population = cell(population_size, 1);
    for i = 1:population_size
        individual = crossed_population{i};
        if rand < 0.1 % 变异概率为0.1
            idx1 = randi(numel(individual));
            idx2 = randi(numel(individual));
            individual([idx1, idx2]) = individual([idx2, idx1]);
        end
        mutated_population{i} = individual;
    end
end
